#include "scara.h"
#include "omni.h"
#include <iostream>
#include <cmath>
#include <cstring>
#include <cstdio>   /* Standard input/output definitions */
#include <cstdlib>
#include "cv.h"
#include "highgui.h"
#include "unistd.h"

using namespace std;

#define STEP 0.5 // em cm
#define MM 100 // em mm

scara myScara(ARDUINO_SCARA); //cria um objeto scara e linka com o arduinoScara
omni myOmni(ARDUINO_OMNI);

bool thread_main_iniciada = false;
pthread_t t_main, t_timeOut;


void* thread_main(void*)
{

  float X = 17.0,Y = 0.0,Z = 0.0,orientation = 0.0; // POSICAO INICIAL DO SCARA
  int leitura = 0;

  cout << "Esperando Start..." << endl;
  
  // While( arduino nao der start ) { esperar }
    myScara.esperaStart();

  cout << "START DADO!" << endl;
  //myScara.flush(); // apaga as baboseiras da serial...
  
  cvNamedWindow( "joystickScara", 1 );
  //cvShowImage( "joystickScara",NULL);
  int key = -1;

  char leituraSensor[10]; 

  myScara.scaraGoTo(X,Y,orientation);

  thread_main_iniciada = true; // agora a thread_timeOut comeca a trabalhar...

while( key != 262243 ) // ctrl+c
{
	key = cvWaitKey(50);

	if( key != -1 )	
	{
		switch(key)
		{
			case 'w': cout << "Omni pra frente" << endl;
				  myOmni.go(FRENTE,MM);
			break;

			case 's': cout << "Omni pra tras" << endl;
				  myOmni.go(TRAS,MM);
			break;

			case 'a': cout << "Omni pra esquerda" << endl;
				  myOmni.go(ESQ,MM);
				  
			break;

			case 'd': cout << "Omni pra direita" << endl;
				  myOmni.go(DIR,MM);
			break;

			case 'q': cout << "Omni gira sent. horario" << endl;
				  myOmni.turn(CW,90);
				  
			break;

			case 'e': cout << "Omni gira sent. anti-horario" << endl;
				  myOmni.turn(CCW,90);
			break;
 
/*			case 'p': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(FRENTE,EXTERNO);
			break;	  
			
			case 'o': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(FRENTE,INTERNO);
			break;	  
				  
			case 'i': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(TRAS,EXTERNO);
			break;
			
			case 'u': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(TRAS,INTERNO);
			break;
			
			case 'l': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(LAT_DIR,EXTERNO);
			break;
			
			case 'k': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(LAT_DIR,INTERNO);
			break;
			
			case 'j': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(LAT_ESQ,EXTERNO);
			break;
			
			case 'h': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(LAT_ESQ,INTERNO);
				  
			break;
			
			case 'm': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(MEIO,EXTERNO);
				  
			break;
			
			case 'n': cout << "Alinhando omni... " << endl;
				  myOmni.alinhaIR(MEIO,INTERNO);
				  
			break; */

			case 65362:
				cout << "X++" << endl;
				X = X + STEP;
				if( myScara.scaraGoTo(X,Y,orientation) < 0)
				  X = X - STEP;
			break;

			case 65363:
				cout << "Y--" << endl;
				Y = Y - STEP;
				if( myScara.scaraGoTo(X,Y,orientation) < 0)
				  Y = Y + STEP;
			break;

			case 65364:
				cout << "X--" << endl;
				X = X - STEP;
				if( myScara.scaraGoTo(X,Y,orientation) < 0)
				  X = X + STEP;
			break;

			case 65361:
				cout << "Y++" << endl;
				Y = Y + STEP;
				if( myScara.scaraGoTo(X,Y,orientation) < 0)
				  Y = Y - STEP;
			break;

			
			case '=': //botao do +
				cout << "ef++" << endl;
				if( myScara.scaraGoTo(X,Y,++orientation) < 0)
				  orientation--;
			break;
			  
			
			case '-': 
				cout << "ef--" << endl;
				if( myScara.scaraGoTo(X,Y,--orientation) < 0)
				  orientation++;
			break;
			
			case 'p': // P_ega
				myScara.pegaBloco();
				cout << "Pegando o Bloco" << endl;
			break;

			case 'l':
				myScara.deixaBloco();
				cout << "Soltando o Bloco" << endl;
			break;

// 			case 'u': 
// 				myScara.leSensor(0, leituraSensor);
// 				cout << "leituraSensor: " << leituraSensor << endl;
// 			break;

			default:
				printf("tecla: %d\n",key);	
		}
	

		
		cout << "posicao do efetuador: X=" << X << " Y=" << Y << " Theta_ef=" << orientation << endl;
		cvWaitKey(50);
	} 

}
cvDestroyWindow("joystickScara");

}





void* thread_timeOut(void* obj)
{
  
REINICIO:

  long tempoDecorrido = 0;
  struct timeval tNow;
  
  pthread_create(&t_main, 0,thread_main, NULL);
  
  while( thread_main_iniciada == false )
  { usleep(5000); } 
  
  usleep(2000000);
  
  while( true )
  {
    if( myOmni.le_msgEnviada()==1 ) //ha pacotes em transito
    {
      gettimeofday(&tNow, NULL);
      
      tempoDecorrido = tNow.tv_sec - (myOmni.le_t_envio()).tv_sec;
      tempoDecorrido = tempoDecorrido*1000000 + (tNow.tv_usec - (myOmni.le_t_envio()).tv_usec);
            
      if(tempoDecorrido > myOmni.le_timeOut())
      {
	cout << "ESTOUROU O TIME_OUT de " << (myOmni.le_timeOut()) <<"seg!! Hora de retransmitir" << endl;
	myOmni.set_timeOutEstourado(true);
	usleep(2000000);
      }
      
      if( myOmni.le_msgEnviada() == 0 )
	cout << "Tempo de resposta do arduino: " << (float)tempoDecorrido/1000000 << endl;
	
    }
    
    
   /*----------------------- ARDUINO DO SCARA---------------------------*/
   
    if( myScara.le_msgEnviada()==1 ) //ha pacotes em transito
    {
      gettimeofday(&tNow, NULL);
      
      tempoDecorrido = tNow.tv_sec - (myScara.le_t_envio()).tv_sec;
      tempoDecorrido = tempoDecorrido*1000000 + (tNow.tv_usec - (myScara.le_t_envio()).tv_usec);
            
      if(tempoDecorrido > myScara.le_timeOut())
      {
	cout << "ESTOUROU O TIME_OUT de " << (myScara.le_timeOut()) <<"seg!! Hora de retransmitir" << endl;
	myScara.set_timeOutEstourado(true);
	usleep(2000000);
      }
      
      usleep(10000);
      
      if( myScara.le_msgEnviada() == 0 )
	cout << "Tempo de resposta do arduino: " << (float)tempoDecorrido/1000000 << " seg" << endl;
	
    }
    
    if( myScara.le_serial() == 1 ) // REBOOT requerido pelo arduino!
    { 
      //dar um KILL na thread_main e reinicia-la
      cout << "ARDUINO DEU REBOOOOOT!" << endl;
      while( pthread_cancel(t_main) != 0 ) // cancela a thread_main
      { ; }
      
      thread_main_iniciada = false;
      goto REINICIO;
    }
  
  
  usleep(1000000); //thread executa a cada 1seg  
  }
}




int main(void)
{
  
  pthread_create(&t_timeOut, 0,thread_timeOut, NULL);
  
  pthread_join(t_timeOut, NULL);
        
return 0;
}

